#ifndef CONTROL_TASK_H
#define CONTROL_TASK_H
#include "stdint.h"
#include "stdbool.h"
#include "arm_math.h"

typedef enum{
	CHASSMIS_WEAK,
//	CHASSMIS_ON,
	CHASSMIS_SPIN,
	CHASSMIS_FLLOW,
	CHASSMIS_LEG_LOW,
	CHASSMIS_LEG_HIGH,
	CHASSIS_MODE_NUM,
	

}Chassis_Mode_e;

typedef enum{

	Remote_Control_Mode,
	Key_Board_Mode,
	

}Control_Mode_e;

typedef enum{
GIMBAL_WEAK,
	GIMBAL_ON,
	

}Gimbal_Mode_e;


typedef struct{

	float LQR_K[2][6];
  float LQR_X[6];
  float LQR_Output[2][6];
	
  struct
  {
	float L1,L2,L3,L4,L5;
  float L0,L0_dot,Last_L0,Target_L0;
	float Phi1,Phi4;
	float  Phi2,Phi3,Phi0,Last_Phi0;
	float sqrt_A2_B2_C2;
	float Phi0_dot;
	float X_B,Y_B,X_D,Y_D,X_C,Y_C;
	float A0,B0,C0;
  }VMC;
	

	struct{
		float Phi;
		float Phi_dot;
		float Last_Phi;
    float Chassis_Position;
    float Chassis_Velocity;
	
		float Theta;
    float Last_Theta;
    float Theta_dot;
    float Last_Theta_dot;
		
		float Tp;
		float F;
	
	}Measure;

	struct{
	
		float Phi;
		float Phi_dot;
		float Last_Phi;
    float Chassis_Position;
    float Chassis_Velocity;
	
		float Theta;
    float Last_Theta;
    float Theta_dot;
    float Last_Theta_dot;
	
	}Target;
	
	struct
	{	
		float T;
   float FN;
		float Tp;
		float F;
		float P;
		float Zw;
		
	}Moment;
	
	struct{
		
	float T1;
	float T2;
		
	}SendValue;
	
	float w;
	float	Predict_Velocity;
  float	Fusion_Velocity;
	float Phi0_3,Phi1_2,Phi3_2,Phi0_2,Phi3_4;
	
	
}Leg_Info_typedef;



typedef struct{

  uint8_t Chassmis_Mode;
   uint8_t Gimbal_Mode;
	uint8_t Control_Mode;
	uint8_t Leg_Length_Mode;
float K;
float	Chassis_Velocity;
float Chassis_Possion;	
float Chassis_Accel;
float Target_Chassis_Velocity	;
float	Accel;
	struct {
	
		struct{
	uint16_t	Balance_Number_Cnt;
	bool IF_Balance_Init;
		
		}Balance_Init;
		
	}Init;
	
	
float Target_Yaw;
float Yaw_Err;
	
	
	
Leg_Info_typedef L_Leg_Info;
Leg_Info_typedef R_Leg_Info;

}Control_Info_typedef;




//extern Leg_Info_typedef L_Leg_Info;
//extern Leg_Info_typedef R_Leg_Info;
extern Control_Info_typedef Control_Info;








#endif